//------------------------------------------------------------------------------ 
//  <copyright file="ControllerClientMessageHandler.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------	

#pragma once

#include "stdafx.h"
#include "SharedBufferParameters.h"
#include "ClientMessages.h"
#include "ControllerClientMessageHandler.h"
#include "MessageDescription.h"
#include "KukaLBR4Tracing.h"

#include <KukaLBR4Definitions.h>
#include <ControllerStateMachineRegisters.h>
#include <KukaLbr4Operations.Manifest.h>

__int64 lastCsmdStartTime = 0;

using namespace KukaLBR4ControllerCommonNS;

/// <summary>
/// Initializes a new insance of the <c>ControllerClientMessageHandler</c> class.
/// Objects of this type are intended to be returned upon creation of 
/// a <c>ControllerStateMachineExecutive</c> object to allow the creating component
/// (i.e. the upstream client) to send commands and get feedback from the
/// <c>ControllerSTateMachineExecutive</c>.
/// <summary>
/// <param name="sbParam">
/// A pointer to a <c>ThreadParams</c> that contains the necessary
/// parameters for the thread, including memory locations for performing pointer exchanges
/// with the controller thread.
/// </param>
ControllerClientMessageHandler::ControllerClientMessageHandler(SharedBufferParameters *pSbParams)
{
	this->pAllRegistersExchange = pSbParams->pAllRegistersExchange;
	this->pWorkingRegistersExchange = pSbParams->pWorkingRegistersExchange;
	this->pCsmdExchange = pSbParams->pCsmdExchange;

	// signals used for indicating when new data is available in an exchange buffer,
	// or when an event occurs that the thread needs to respond to (such as the other
	// thread quitting).
	this->pQuitSignal = pSbParams->pQuitSignal;
	this->pCsmdStatusSignal = pSbParams->pCsmdStatusSignal;
	this->pNewCsmdReadySignal = pSbParams->pNewCsmdReadySignal;

	// allocate private copy of working and all registers
	this->pAllRegisters = new ControllerStateMachineRegisters<double> (
		KukaLBR4PlusFeedbackRegisters::NumberOfFeedbackRegisters + KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters,
		(unsigned int *)KukaLBR4PlusRegisterSizes);

	this->pWorkingRegisters = new ControllerStateMachineRegisters<double> (
		KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters,
		(unsigned int *)KukaLBR4PlusRegisterSizes);

	unsigned int csmdSize = sizeof(ControllerStateMachineDescription);
	this->pCsmdRegister = new ControllerStateMachineRegisters<byte> (
		1,
		&csmdSize);
		
	// ensure we don't send uninitialized feedback to the upstream client
	this->haveValidFeedback = false;

	// ensure we don't try to execute CSM before we have a valid machine description
	this->csmdInitialized = false;
}

/// <summary> dtor to make sure quit signal has been indicated to controller thread </summary>
ControllerClientMessageHandler::~ControllerClientMessageHandler()
{
	// set the quit signal (harmless if already set)
	InterlockedExchange(this->pQuitSignal, SignalState::Signaled);
}

void ControllerClientMessageHandler::ProcessMessage(MessageDescription *pMessage, MessageDescription *pResponse)
{
	pResponse->MessageType = MessageTypes::InvalidMessageType;

	// see if the control thread indicated we should exit
	if(InterlockedExchange(this->pQuitSignal, SignalState::NotSignaled) == SignalState::Signaled)
	{
		return;
	}

	if(pMessage->MessageType == MessageTypes::ConnectionClosedMessageType)
	{
		// first set quit signal
		InterlockedExchange(this->pQuitSignal, SignalState::Signaled);
		// we do not respond to close connection requests
		return;
	}

	switch(pMessage->MessageType)
	{
	case MessageTypes::SetCsmdMessageType:
		// spin lock on being clear to write to Csmd and Working register exchanges
		// this should never block for any appreciable length of time (<< 1ms)
		while(InterlockedCompareExchange(this->pNewCsmdReadySignal, CsmdReadyStates::NotReady, CsmdReadyStates::Ready) == CsmdReadyStates::Consuming); 

		this->SetCsmd(pMessage);
		this->csmdInitialized = true;
		this->BuildDefaultMessageResponse(
			MessageTypes::SetCsmdMessageType,
			pMessage->SequenceNumber,
			true,
			pResponse);
		break;

	case MessageTypes::SetWorkingRegistersMessageType:
		// spin lock on being clear to write to Csmd and Working register exchanges
		// this should never block for any appreciable length of time (<< 1ms)
		while(InterlockedCompareExchange(this->pNewCsmdReadySignal, CsmdReadyStates::NotReady, CsmdReadyStates::Ready) == CsmdReadyStates::Consuming); 

		this->SetNewWorkingRegistersFromMessage(pMessage);
		this->BuildDefaultMessageResponse(
			MessageTypes::SetWorkingRegistersMessageType,
			pMessage->SequenceNumber,
			true,
			pResponse);
		break;

	case MessageTypes::StartCsmdMessageType:
		{
			bool startCsmdSuccess = false;
			if(this->csmdInitialized)
			{
				InterlockedExchange(this->pCsmdStatusSignal, ControllerStatus::Running);
				while(InterlockedCompareExchange(this->pNewCsmdReadySignal, CsmdReadyStates::Ready, CsmdReadyStates::NotReady) == CsmdReadyStates::Consuming); 
				startCsmdSuccess = true;
			}

			__int64 currentTime;

			QueryPerformanceCounter((LARGE_INTEGER *)&currentTime);

			if(lastCsmdStartTime != 0) 
			{
				KukaLBR4Tracing::WriteTimingEvent(currentTime, lastCsmdStartTime, ClientMessageHandlerIssueCsmdStart, KukaLBR4Tracing::TimingEventTypes::RecvFriStatus);
			}

			lastCsmdStartTime = currentTime;

			BuildDefaultMessageResponse(
				MessageTypes::StartCsmdMessageType,
				pMessage->SequenceNumber,
				startCsmdSuccess,
				pResponse);
			break;
		}

	case MessageTypes::StopCsmdMessageType:
		InterlockedExchange(this->pCsmdStatusSignal, ControllerStatus::Stopped);
		BuildDefaultMessageResponse(
			MessageTypes::StopCsmdMessageType,
			pMessage->SequenceNumber,
			true,
			pResponse);
		break;

	case MessageTypes::GetFeedbackMessageType:
		// if there is new feedback available from the control thread, then get it
		if(this->pAllRegistersExchange->Consume(this->pAllRegisters))
		{
			this->haveValidFeedback = true;
		}
		
		BuildFeedbackMessageResponse(
			pMessage->SequenceNumber,
			this->haveValidFeedback,
			pResponse);
		break; 

	default:
		throw "Unsupported message type recieved";
	}

}

/// <summary>
/// Replaces the current set of working registers in the controller state machine with a new 
/// set of values.
/// </summary>
/// <param name="pMessage"> Message containing the new working register values. </param>
void ControllerClientMessageHandler::SetNewWorkingRegistersFromMessage(MessageDescription *pMessage)
{
	if (pMessage->MessageType != MessageTypes::SetWorkingRegistersMessageType)
	{
		throw "Expected SetMachineRegistersMessageType";
	}

	if(pMessage->ParameterLength != sizeof(KukaWorkingRegisterMessage))
	{
		throw "Invalid parameter length for SetMachineRegistersMessageType";
	}

	unsigned int i = 0;
	unsigned int currentPosition = 0;

	// get array of pointers to private copy of working registers
	// copy values from the message into the working registers 
	// and then publish them for the controller thread
	double **registers = this->pWorkingRegisters->GetRegisters();

	for(i = 0; i < KukaLBR4PlusWorkingRegisters::NumberOfWorkingRegisters; ++i)
	{
		memcpy_s(
			registers[i],
			KukaLBR4PlusRegisterSizes[i],
			&(pMessage->pParameters[currentPosition]),
			KukaLBR4PlusRegisterSizes[i]);

		currentPosition += KukaLBR4PlusRegisterSizes[i];
	}

	this->pWorkingRegistersExchange->Publish(this->pWorkingRegisters);
}

/// <summary>
/// Replaces the current state machine defintion for the controller state machine.
/// </summary>
/// <param name="pMessage"> Message containing the new controller state machine description. </param>
void ControllerClientMessageHandler::SetCsmd(MessageDescription *pMessage)
{
	if(pMessage->MessageType != MessageTypes::SetCsmdMessageType)
	{
		throw "Expected SetCsmdMessageType message";
	}

	if(pMessage->ParameterLength != sizeof(ControllerStateMachineDescription) || pMessage->pParameters == NULL)
	{
		throw "Invalid parameter length for SetCsmdMessageType";
	}

	memcpy_s(
		this->pCsmdRegister->GetRegister(0),
		sizeof(ControllerStateMachineDescription),
		pMessage->pParameters,
		pMessage->ParameterLength);

	this->pCsmdExchange->Publish(this->pCsmdRegister);
}

/// <summary> Build a standard response message to a request from the upstream client. </summary>
/// <param name="messageType"> Type of message to respond to. </param>
/// <param name="sequenceNumber"> Sequence number of the message to respond to. </param>
/// <param name="successful"> Indicates whether or not the the response should indicate success. </param>
/// <param name="pResponse"> Pointer to a preallocated response message. </param>
void ControllerClientMessageHandler::BuildDefaultMessageResponse(
	MessageTypes messageType,
	unsigned int sequenceNumber,
	bool successful,
	MessageDescription *pResponse)
{
	if(pResponse->ParameterBufferLength < 1)
	{
		throw "Parameter buffer size insufficient";
	}

	pResponse->MessageType = messageType,
		pResponse->SequenceNumber = sequenceNumber;
	pResponse->IsResponse = true;
	pResponse->ParameterLength = 1;
	if(successful)
	{
		pResponse->pParameters[0] = 1;
	}
	else
	{
		pResponse->pParameters[0] = 0;
	}
}

/// <summary> Build a feedback response message for the upstream client. </summary>
/// <param name="sequenceNumber"> Sequence number of the message to respond to. </param>
/// <param name="successful"> Indicates whether or not the the response should indicate success. </param>
/// <param name="pResponse"> Pointer to a preallocated response message. </param>
void ControllerClientMessageHandler::BuildFeedbackMessageResponse(
	unsigned int sequenceNumber,
	bool successful,
	MessageDescription *pResponse)
{
	if(pResponse->ParameterBufferLength < sizeof(KukaFeedbackMessage))
	{
		throw "Parameter buffer size insufficient";
	}

	pResponse->MessageType = MessageTypes::GetFeedbackMessageType;
	pResponse->SequenceNumber = sequenceNumber;
	pResponse->IsResponse = 1;
	if(successful)
	{
		pResponse->ParameterLength = sizeof(KukaFeedbackMessage);
		int currentPosition = 0;
		double **allRegisters = this->pAllRegisters->GetRegisters();
		unsigned int *registerSizes = this->pAllRegisters->GetRegisterSizesInBytes();

		for(int i = 0; i < KukaLBR4PlusTotalNumberOfRegisters; i++)
		{
			memcpy_s(
				&(pResponse->pParameters[currentPosition]),
				pResponse->ParameterBufferLength - currentPosition,
				allRegisters[i],
				registerSizes[i]);

			currentPosition += registerSizes[i];
		}
	}
	else
	{
		pResponse->ParameterLength = 0;
	}
}
